#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include "control_base/WMJRobotControl.h"
#include "base_interfaces/action/navigation.hpp"


// #define __NAVIGATION_DEBUG__

namespace wmj
{

    using Navigation = base_interfaces::action::Navigation;
    using GoalHandleNavigation = rclcpp_action::ClientGoalHandle<Navigation>;
    using namespace std::placeholders;

    class ChassisManagement : public rclcpp::Node
    {
    private:
        rclcpp_action::Client<Navigation>::SharedPtr m_navigationActionClient;
        std::shared_ptr<wmj::WMJRobotControl> m_robotControl;
        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr m_goalReceiver;

        geometry_msgs::msg::Pose m_goal;
    public:

        ChassisManagement(std::string name, std::shared_ptr<wmj::WMJRobotControl> robotControl = nullptr)
        : Node(name)
        {
            if(!robotControl){
                m_robotControl = std::make_shared<wmj::WMJRobotControl>();
            }
            m_navigationActionClient = rclcpp_action::create_client<Navigation>(this, "Navigation");
            m_goalReceiver = this->create_subscription<geometry_msgs::msg::PoseStamped>(
                "goal", 5, std::bind(&ChassisManagement::recvGoal, this, _1));
        }
        ~ChassisManagement(){
            if(m_robotControl){
                m_robotControl->setChassisSpeed(0, 0);
                m_robotControl->SetGimbalSpeed(0, 0);
                m_robotControl->top(false);
                m_robotControl->ShootNone();
            }
        }

        /**
         * 接受目标点
         */
        void recvGoal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
        {
#ifdef __NAVIGATION_DEBUG__
            RCLCPP_INFO(this->get_logger(), "Received goal: w=%lf x=%lf y=%lf z=%lf", 
                        msg->pose.orientation.w, msg->pose.orientation.x, 
                        msg->pose.orientation.y, msg->pose.orientation.z);
#endif //__NAVIGATION_DEBUG__
            m_goal = msg->pose;
            this->sendGoal();
        }

        /**
         * Action客户端,创建一个action并发送goal
         */
        void sendGoal()
        {
            if(!m_navigationActionClient->wait_for_action_server(std::chrono::seconds(5)))
            {
                RCLCPP_ERROR(this->get_logger(), "Action Server is not available");
                return;
            }

            Navigation::Goal goal_msg;
            goal_msg.pose.pose = m_goal; 

#ifdef __NAVIGATION_DEBUG__
            RCLCPP_INFO(this->get_logger(), "Send Goal: psoe: x=%lf, y=%lf, z=%lf, w=%lf", 
                                goal_msg.pose.pose.orientation.x, goal_msg.pose.pose.orientation.y, 
                                goal_msg.pose.pose.orientation.z, goal_msg.pose.pose.orientation.w);
#endif
            auto send_goal_options = 
                rclcpp_action::Client<Navigation>::SendGoalOptions();
            send_goal_options.goal_response_callback =
                std::bind(&ChassisManagement::goal_response_callback, this, _1);
            send_goal_options.feedback_callback =
                std::bind(&ChassisManagement::feedback_callback, this, _1, _2);
            send_goal_options.result_callback =
                std::bind(&ChassisManagement::result_callback, this, _1);
            m_navigationActionClient->async_send_goal(goal_msg, send_goal_options);
        }

        /**
         * Action回调函数组 
         */

        void goal_response_callback(std::shared_future<GoalHandleNavigation::SharedPtr> future) {
            auto goal_handel = future.get();
            if (!goal_handel) {
                RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
            } 
            else {
                RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
            }
        }
        /**
         * @details feedback承担将接受到的导航信息转发给底盘的任务。
         * 导航信息的feedback分为三项，分别为 x方向速度 y方向速度 yaw方向角速度
         * 其中 yaw方向角速度 如果为单云台则为云台角速度，如果为双云台则为大yaw轴角速度
         */
        void feedback_callback(
            GoalHandleNavigation::SharedPtr,
            const std::shared_ptr<const Navigation::Feedback> feedback) 
        {
            double x = feedback->x;
            double y = feedback->y;
            double w = feedback->w;
            if(m_robotControl){
                m_robotControl->setChassisSpeed(x, y);
                //! 此处，如果单云台则为云台角速度，如果为双云台则为大yaw轴角速度
                m_robotControl->SetGimbalSpeed(m_robotControl->GetGimbalSpeed().pitch, w);
                //
                RCLCPP_INFO(this->get_logger(), "Set chassis speed: x=%lf y=%lf w=%lf", x, y, w);
            }
            else
                RCLCPP_INFO(this->get_logger(), "m_robotControl is NULL");
        }
        /**
         * @brief 收到action结束信号后，结束导航，底盘速度和角速度归零。
         */
        void result_callback(const GoalHandleNavigation::WrappedResult& result) {
            switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "Goal was success");
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
                return;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
                return;
            default:
                RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                return;
            }
            if(m_robotControl){
                m_robotControl->setChassisSpeed(0.0, 0.0);
                m_robotControl->SetGimbalSpeed(0.0, 0.0);
            }
            RCLCPP_INFO(this->get_logger(), "Result received: %f", result.result->success);
        }
    };

}